/*************************************************************************
Crytek Source File.
Copyright (C), Crytek Studios, 2001-2009.
-------------------------------------------------------------------------
$Id$
$DateTime$

-------------------------------------------------------------------------
History:
- 4:3:2009   11:38 : Created by Marcio Martins

*************************************************************************/
#include "StdAfx.h"
#include "VisionMap.h"

#include "DebugDrawContext.h"


const float CVisionMap::PositionEpsilon = 0.05f;
const float CVisionMap::OrientationEpsilon = 0.05f;


CVisionMap::CVisionMap()
:	m_pvsUpdateCount(0)
, m_visUpdateCount(0)
, m_genID(0)
{
	gEnv->pEntitySystem->AddSink(this, IEntitySystem::OnEvent, 1 << ENTITY_EVENT_XFORM);
}

CVisionMap::~CVisionMap()
{
	gEnv->pEntitySystem->RemoveSink(this);
}

bool CVisionMap::OnBeforeSpawn(SEntitySpawnParams& params)
{
	return true;
}

void CVisionMap::OnSpawn(IEntity* pEntity,SEntitySpawnParams& params)
{
}

bool CVisionMap::OnRemove(IEntity* pEntity)
{
	return true;
}

void CVisionMap::OnReused(IEntity *pEntity, SEntitySpawnParams &params)
{
}

void CVisionMap::OnEvent(IEntity* pEntity, SEntityEvent& event)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	IPhysicalEntity* pPhysics = pEntity->GetPhysics();

	pe_type type = pPhysics ? pPhysics->GetType() : PE_NONE;
	
	if ((type != PE_NONE) && (type != PE_PARTICLE) && (type != PE_ROPE))
	{
		pe_status_dynamics dynamics;
		if (pPhysics->GetStatus(&dynamics))
		{
			if (dynamics.v.len2() > square(15.f))
				return;
		}

		pe_params_part part;
		part.partid = 0;

		if (pPhysics->GetParams(&part))
		{
			phys_geometry* pPhysGeom = part.pPhysGeom;
			if (pPhysGeom)
			{
				if (pPhysGeom->V <= cube(0.35f))
					return;
			}
			else
			{
				phys_geometry* pPhysGeomProxy = part.pPhysGeomProxy;
				if (part.pPhysGeomProxy->V <= cube(0.35f))
					return;
			}
		}

		Vec3 pos = pEntity->GetWorldPos();

		Observers::iterator obsIt = m_observers.begin();
		Observers::iterator end = m_observers.end();

		for ( ; obsIt != end; ++obsIt)
		{
			ObserverInfo& observerInfo = obsIt->second;

			// PVS is getting updated anyway so ignore
			if (observerInfo.dirtyPVS || observerInfo.dirtyVis)
				continue;

			ObserverParams& observerParams = observerInfo.params;

			const Vec3& eyePos = observerParams.eyePos;

			float distanceSq = (pos - eyePos).len2();

			if (distanceSq > square(observerParams.sightRange))
				continue;

			if (!IsPointInFoV(eyePos, observerParams.eyeDir, pos, observerParams.peripheralFoVCos, distanceSq))
				continue;

			observerInfo.dirtyVis = true;
		}
	}
}

void CVisionMap::Reset()
{
	COMPILE_TIME_ASSERT((ObserverParams::MaxSkipListSize + ObservableParams::MaxSkipListSize)
		<= RayCastRequest::MaxSkipListCount);

	{
		Observers::iterator it = m_observers.begin();
		Observers::iterator end = m_observers.end();

		for ( ; it != end; ++it)
		{
			ObserverInfo& info = it->second;

			ReleaseSkipList(&info.params.skipList[0], info.params.skipListSize);
			DeletePendingRays(it->second.pvs);
		}
	}

	{
		Observables::iterator it = m_observables.begin();
		Observables::iterator end = m_observables.end();

		for ( ; it != end; ++it)
		{
			ObservableInfo& info = it->second;

			ReleaseSkipList(&info.params.skipList[0], info.params.skipListSize);
		}
	}

	m_pvsUpdateCount = 0;
	m_visUpdateCount = 0;

	m_observers.clear();

	m_observablesGrid.clear();
	m_observables.clear();
	m_pendingRays.clear();
}

VisionID CVisionMap::CreateVisionID(const char* name)
{
	++m_genID;
	while (!m_genID)
		++m_genID;

	return VisionID(++m_genID, name);
}

void CVisionMap::RegisterObserver(const ObserverID& observerID, const ObserverParams& params)
{
	if (!observerID)
		return;

	m_observers.insert(Observers::value_type(observerID, ObserverInfo()));

	ObserverChanged(observerID, params, eChangedAll);
}

void CVisionMap::UnregisterObserver(const ObserverID& observerID)
{
	if (!observerID)
		return;

	Observers::iterator it = m_observers.find(observerID);
	if (it == m_observers.end())
		return;

	ObserverInfo& info = it->second;
	ReleaseSkipList(&info.params.skipList[0], info.params.skipListSize);
	DeletePendingRays(it->second.pvs);

	m_observers.erase(it);
}

void CVisionMap::RegisterObservable(const ObservableID& observableID, const ObservableParams& params)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	if (!observableID)
		return;

	assert(params.posCount > 0);
	assert(params.posCount <= ObservableParams::MaxPositionCount);

	std::pair<Observables::iterator, bool> result = m_observables.insert(
		Observables::value_type(observableID, ObservableInfo(observableID, ObservableParams())));

	ObservableInfo& info = result.first->second;
	m_observablesGrid.insert(info.params.pos[0], &info);
	
	ObservableChanged(observableID, params, eChangedAll);
}

void CVisionMap::UnregisterObservable(const ObservableID& observableID)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	if (!observableID)
		return;

	Observables::iterator obsIt = m_observables.find(observableID);
	if (obsIt == m_observables.end())
		return;

	ObservableInfo& info = obsIt->second;
	ReleaseSkipList(&info.params.skipList[0], info.params.skipListSize);

	Observers::iterator it = m_observers.begin();
	Observers::iterator end = m_observers.end();

	for ( ; it != end; ++it)
	{
		if (it->first == observableID)
			continue;

		ObserverInfo& observerInfo = it->second;

		PVS::iterator pvsIt = observerInfo.pvs.find(observableID);
		if (pvsIt == observerInfo.pvs.end())
			continue;
			
		PVSEntry& entry = pvsIt->second;
		if (entry.visible && observerInfo.params.callback)
			observerInfo.params.callback(it->first, observerInfo.params, 
				observableID, entry.observableInfo.params, false);

		DeletePendingRay(pvsIt->second);

		observerInfo.pvs.erase(pvsIt);
		observerInfo.dirtyPVS = true;
	}

	m_observablesGrid.erase(info.params.pos[0], &info);
	m_observables.erase(obsIt);
}

void CVisionMap::ObserverChanged(const ObserverID& observerID, const ObserverParams& params, uint32 hint)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	Observers::iterator it = m_observers.find(observerID);
	assert(it != m_observers.end());
	if (it == m_observers.end())
		return;

	bool needsUpdate = false;
	ObserverInfo& observerInfo = it->second;
	ObserverParams& myparams = observerInfo.params;

	// if only priority changed
	if (hint & eChangedPriority)
	{
		myparams.priority = params.priority;
	}
	
	if (hint & eChangedFaction)
	{
		myparams.factionMask = params.factionMask;
		needsUpdate = true;
	}

	if (hint & eChangedSight)
	{
		myparams.sightRange = params.sightRange;
		myparams.primaryFoVCos = params.primaryFoVCos;
		myparams.peripheralFoVCos = params.peripheralFoVCos;
		needsUpdate = true;
	}

	if (hint & eChangedPosition)
	{
		if (!IsEquivalent(myparams.eyePos, params.eyePos, PositionEpsilon))
		{
			myparams.eyePos = params.eyePos;
			needsUpdate = true;
		}
	}

	if (hint & eChangedOrientation)
	{
		if (!IsEquivalent(myparams.eyeDir, params.eyeDir, OrientationEpsilon))
		{
			myparams.eyeDir = params.eyeDir;
			needsUpdate = true;
		}
	}

	if (hint & eChangedSkipList)
	{
#if defined(LINUX) && !defined(_DEBUG) // g++ always issues error(comparison is always true due to limited range of data type)
		assert(params.skipListSize <= ObserverParams::MaxSkipListSize);
#endif

		uint32 skipListSize = std::min<uint32>(params.skipListSize, ObserverParams::MaxSkipListSize);
		AcquireSkipList(const_cast<IPhysicalEntity**>(&params.skipList[0]), skipListSize);
		ReleaseSkipList(&myparams.skipList[0], myparams.skipListSize);

		myparams.skipListSize = skipListSize;
		for (int i = 0; i < myparams.skipListSize; ++i)
			myparams.skipList[i] = params.skipList[i];

#ifdef _DEBUG
		std::sort(&myparams.skipList[0], &myparams.skipList[myparams.skipListSize]);
		
		for(uint i = 1; i < myparams.skipListSize; ++i)
		{
			assert(myparams.skipList[i - 1] != myparams.skipList[i]);
		}
#endif
	}

	if (hint & eChangedCallback)
	{
		myparams.callback = params.callback;
	}

	if (hint & eChangedUserData)
	{
		myparams.userData = params.userData;
	}

	if (hint & eChangedEntityId)
	{
		myparams.entityID = params.entityID;
	}

	if (hint & eChangedTypeMask)
	{
		myparams.typeMask = params.typeMask;
		needsUpdate = true;
	}

	if (needsUpdate)
		observerInfo.dirtyPVS = true;
}

void CVisionMap::ObservableChanged(const ObservableID& observableID, const ObservableParams& params, uint32 hint)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	Observables::iterator observableIt = m_observables.find(observableID);
	assert(observableIt != m_observables.end());
	if (observableIt == m_observables.end())
		return;

	ObservableInfo& observableInfo = observableIt->second;
	ObservableParams& observableParams = observableInfo.params;

	bool visibilityChanged = false;
	if (hint & eChangedPosition)
	{
		assert(params.posCount > 0);
		assert(params.posCount <= ObservableParams::MaxPositionCount);
		assert(params.pos[0].IsValid());

		Vec3 oldpos = observableParams.pos[0];

		if (!IsEquivalent(oldpos, params.pos[0], PositionEpsilon))
		{
			FRAME_PROFILER("ObservableChanged_UpdateHashGrid", GetISystem(), PROFILE_AI);
		
			observableParams.pos[0] = params.pos[0];
			m_observablesGrid.move(m_observablesGrid.find(oldpos, &observableInfo), params.pos[0]);
			visibilityChanged = true;
			
			observableParams.posCount = params.posCount;

			for (int i = 1; i < observableParams.posCount; ++i)
			{
				assert(params.pos[i].IsValid());
				observableParams.pos[i] = params.pos[i];
			}
		}
	}

	if (hint & eChangedUserData)
	{
		observableParams.userData = params.userData;
	}

	if (hint & eChangedCallback)
	{
		observableParams.callback = params.callback;
	}

	if (hint & eChangedCamouflage)
	{
		observableParams.camouflage = params.camouflage;
		visibilityChanged = true;
	}

	if (hint & eChangedSkipList)
	{
		assert(params.skipListSize <= ObserverParams::MaxSkipListSize);

		uint32 skipListSize = std::min<uint32>(params.skipListSize, ObserverParams::MaxSkipListSize);
		AcquireSkipList(const_cast<IPhysicalEntity**>(&params.skipList[0]), skipListSize);
		ReleaseSkipList(&observableParams.skipList[0], observableParams.skipListSize);

		observableParams.skipListSize = skipListSize;
		for (int i = 0; i < observableParams.skipListSize; ++i)
			observableParams.skipList[i] = params.skipList[i];

#ifdef _DEBUG
		std::sort(&observableParams.skipList[0], &observableParams.skipList[observableParams.skipListSize]);
		
		for(uint i = 1; i < observableParams.skipListSize; ++i)
		{
			assert(observableParams.skipList[i - 1] != observableParams.skipList[i]);
		}
#endif
	}

	if (hint & eChangedFaction)
	{
		observableParams.faction = params.faction;
		visibilityChanged = true;
	}

	if (hint & eChangedTypeMask)
	{
		observableParams.typeMask = params.typeMask;
		visibilityChanged = true;
	}

	if (hint & eChangedEntityId)
	{
		observableParams.entityID = params.entityID;
	}

	if (visibilityChanged)
	{
		Observers::iterator obsIt = m_observers.begin();
		Observers::iterator end = m_observers.end();

		for ( ; obsIt != end; ++obsIt)
		{
			if (obsIt->first == observableID)
				continue;

			// PVS is getting updated anyway so ignore
			if (obsIt->second.dirtyPVS)
				continue;

			ObserverInfo& observerInfo = obsIt->second;
			ObserverParams& observerParams = observerInfo.params;

			const Vec3& eyePos = observerParams.eyePos;

			float rangeSq = sqr(observerParams.sightRange) * sqr(1.0f - observableParams.camouflage);
			float distanceSq = (observableParams.pos[0] - eyePos).len2();

			PVS::iterator pvsIt = observerInfo.pvs.find(observableID);
			bool inPVS = (pvsIt != observerInfo.pvs.end());
			bool visible = inPVS && pvsIt->second.visible;

			bool factionMatch = (observerParams.factionMask & (1 << observableParams.faction)) != 0;
			bool typeMatch = (observerParams.typeMask & observableParams.typeMask) != 0;

			if (!factionMatch || !typeMatch || (distanceSq > rangeSq) || !IsPointInFoV(eyePos, observerParams.eyeDir, 
				observableParams.pos[0], observerParams.peripheralFoVCos, distanceSq))
			{
				if (inPVS)
				{
					FRAME_PROFILER("ObservableChanged_PVSErase1", GetISystem(), PROFILE_AI);

					if (visible)
					{
						if (observerParams.callback)
							observerParams.callback(obsIt->first, observerParams, observableID, observableInfo.params, false);
						if (observableInfo.params.callback)
							observableInfo.params.callback(obsIt->first, observerParams, observableID, observableInfo.params, false);
					}

					DeletePendingRay(pvsIt->second);

					observerInfo.pvs.erase(pvsIt);
					observerInfo.dirtyVis = true;
				}

				continue;
			}

			if (!inPVS)
			{
				FRAME_PROFILER("ObservableChanged_PVSInsert", GetISystem(), PROFILE_AI);

				observerInfo.pvs.insert(PVS::value_type(observableID, PVSEntry(observableInfo)));
			}

			// at this point contents of this pvs have changed for sure
			observerInfo.dirtyVis = true;
		}
	}
}

bool CVisionMap::IsVisible(const ObserverID& observerID, const ObservableID& observableID) const
{
	Observers::const_iterator obsIt = m_observers.find(observerID);
	if (obsIt == m_observers.end())
		return false;

	const ObserverInfo& observerInfo = obsIt->second;
	PVS::const_iterator pvsIt = observerInfo.pvs.find(observableID);
	if (pvsIt == observerInfo.pvs.end())
		return false;

	return pvsIt->second.visible;
}

const ObserverParams* CVisionMap::GetObserverParams(const ObserverID& observerID) const
{
	Observers::const_iterator obsIt = m_observers.find(observerID);
	if (obsIt == m_observers.end())
		return 0;

	const ObserverInfo& observerInfo = obsIt->second;

	return &observerInfo.params;
}

const ObservableParams* CVisionMap::GetObservableParams(const ObservableID& observableID) const
{
	Observables::const_iterator obsIt = m_observables.find(observableID);
	if (obsIt == m_observables.end())
		return 0;

	const ObservableInfo& observableInfo = obsIt->second;

	return &observableInfo.params;
}

void CVisionMap::Update(float frameTime)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	m_pvsUpdateCount = 0;
	m_visUpdateCount = 0;

	CTimeValue now = gEnv->pTimer->GetFrameStartTime();

	Observers::iterator it = m_observers.begin();
	Observers::iterator end = m_observers.end();

	for ( ; it != end; ++it)
	{
		const ObserverID& observerID = it->first;
		ObserverInfo& observerInfo = it->second;
		ObserverParams& observerParams = observerInfo.params;

		int64 period = 100;
		
		switch (observerInfo.params.priority)
		{
		case eLowPriority:
			period = 500;
			break;
		case eMediumPriority:
			period = 400;
			break;
		case eHighPriority:
			period = 333;
		case eVeryHighPriority:
			period = 175;
			break;
		default:
			assert(0);
			break;
		};

		bool dirtyPVS = observerInfo.dirtyPVS;
		bool dirtyVis = observerInfo.dirtyVis;

		if (dirtyPVS && ((now - observerInfo.pvsUpdated).GetMilliSecondsAsInt64() >= period))
		{
			UpdatePVS(observerID, observerInfo, now);

			observerInfo.dirtyPVS = false;
			observerInfo.pvsUpdated = now;
			dirtyVis = true;

			++m_pvsUpdateCount;
		}

		if (dirtyVis && ((now - observerInfo.visUpdated).GetMilliSecondsAsInt64() >= period))
		{
			UpdateVisibility(observerID, observerInfo, now);

			observerInfo.dirtyVis = false;
			observerInfo.visUpdated = now;

			++m_visUpdateCount;
		}
	}
}

void CVisionMap::UpdatePVS(const ObserverID& observerID, ObserverInfo& observerInfo, const CTimeValue& now)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	ObserverParams& params = observerInfo.params;

	const Vec3 eyePos = params.eyePos;
	const Vec3 eyeDir = params.eyeDir;

	float baseSightRangeSq = sqr(params.sightRange);
	PVS& pvs = observerInfo.pvs;

	assert(eyeDir.IsUnit());
	assert(eyePos.IsValid());

	// TODO: Think of a better way to do the following:
	// (It's needed because we want to keep the state of the visibles for the remaining of this frame, until new results come in)

	// Step1:
	//	-Make sure everything in the PVS is in supposed to be there
	//		-Delete what it's not

	// Step2:
	//	-Go through all objects in range
	//		-If object is already in the PVS skip it
	//		-Otherwise check if it should be added and add it

	{
		FRAME_PROFILER("UpdatePVS_Step1", GetISystem(), PROFILE_AI);

		PVS::iterator pvsIt = pvs.begin();
		PVS::iterator end = pvs.end();

		for ( ; pvsIt != end; )
		{
			const ObservableID& observableID = pvsIt->first;
			const ObservableParams& observableParams = pvsIt->second.observableInfo.params;
			PVSEntry& pvsEntry = pvsIt->second;

			const Vec3& pos = observableParams.pos[0];
			bool factionMatch = (params.factionMask & (1 << observableParams.faction)) != 0;
			bool typeMatch = (params.typeMask & observableParams.typeMask) != 0;

			float sightRangeSq = baseSightRangeSq * sqr(1.0f - observableParams.camouflage);

			float distanceSq = (pos - eyePos).len2();

			if (!typeMatch || !factionMatch || (distanceSq > sightRangeSq) ||
				!IsPointInFoV(eyePos, eyeDir, pos, params.peripheralFoVCos, distanceSq))
			{
				PVS::iterator next = pvsIt;
				++next;

				if (pvsEntry.visible)
				{
					const ObservableParams& observableEntryParams = pvsEntry.observableInfo.params;
					if (params.callback)
						params.callback(observerID, params, observableID, observableEntryParams, false);

					if (observableEntryParams.callback)
						observableEntryParams.callback(observerID, params, observableID, observableEntryParams, false);
				}

				DeletePendingRay(pvsEntry);

				pvs.erase(pvsIt);
				pvsIt = next;

				continue;
			}

			++pvsIt;
		}
	}

	{
		FRAME_PROFILER("UpdatePVS_Step2", GetISystem(), PROFILE_AI);

		m_queryObservables.clear();
		uint32 observableCount = m_observablesGrid.query_sphere_distance(eyePos, params.sightRange, m_queryObservables);

		QueryObservables::iterator it = m_queryObservables.begin();
		QueryObservables::iterator end = m_queryObservables.end();

		for ( ; it != end; ++it)
		{
			ObservableInfo* observableInfo = it->second;

			if (observableInfo->id == observerID)
				continue;

			PVS::iterator pvsIt = pvs.find(observableInfo->id);
			if (pvsIt != pvs.end())
				continue;

			const ObservableParams& observableParams = observableInfo->params;
			bool factionMatch = (params.factionMask & (1 << observableParams.faction)) != 0;
			bool typeMatch = (params.typeMask & observableParams.typeMask) != 0;

			if (!factionMatch || !typeMatch)
					continue;

			float distanceSq = it->first;
			float sightRangeSq = baseSightRangeSq * sqr(1.0f - observableParams.camouflage);

			if ((distanceSq <= sightRangeSq) &&
				IsPointInFoV(eyePos, eyeDir, observableParams.pos[0], params.peripheralFoVCos, distanceSq))
			{
				assert(m_observables.find(observableInfo->id) != m_observables.end());	// Consistency check

				std::pair<PVS::iterator, bool> result = pvs.insert(PVS::value_type(observableInfo->id, PVSEntry(*observableInfo)));
				assert(result.second);
			}
		}
	}
}

void CVisionMap::UpdateVisibility(const ObserverID& observerID, ObserverInfo& observerInfo, const CTimeValue& now)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	PVS& pvs = observerInfo.pvs;

	PVS::iterator pvsIt = pvs.begin();
	PVS::iterator end = pvs.end();

	for ( ; pvsIt != end; ++pvsIt)
	{
		assert(pvsIt->first != observerID);

		PVSEntry& entry = pvsIt->second;
		if (!entry.pendingRayID)
			entry.pendingRayID = QueueRay(observerID, observerInfo, entry);
	}
}

QueuedRayID CVisionMap::QueueRay(const ObserverID& observerID, const ObserverInfo& observerInfo, PVSEntry& entry)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	assert(!entry.pendingRayID);

	RayCastRequest::Priority priority;

	switch (observerInfo.params.priority)
	{
	case eLowPriority:
		priority = RayCastRequest::LowPriority;
		break;
	case eMediumPriority:
		priority = RayCastRequest::MediumPriority;
		break;
	case eHighPriority:
		priority = RayCastRequest::HighPriority;
		break;
	case eVeryHighPriority:
		priority = RayCastRequest::HighestPriority;
		break;
	default:
		assert(0);
		break;
	}

	QueuedRayID rayID = gAIEnv.pRayCaster->Queue(priority, functor(*this, &CVisionMap::RayCastComplete),
		functor(*this, &CVisionMap::RayCastSubmit));

	std::pair<PendingRays::iterator, bool> result = m_pendingRays.insert(
		PendingRays::value_type(rayID, PendingRayInfo(observerID, observerInfo, entry)));
	assert(result.second);

	return rayID;
}

void CVisionMap::DeletePendingRay(PVSEntry& entry)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	if (entry.pendingRayID)
	{
		gAIEnv.pRayCaster->Cancel(entry.pendingRayID);
		m_pendingRays.erase(entry.pendingRayID);
		entry.pendingRayID = 0;
	}
}

void CVisionMap::DeletePendingRays(PVS& pvs)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	PVS::iterator pvsIt = pvs.begin();
	PVS::iterator end = pvs.end();

	for ( ; pvsIt != end; ++pvsIt)
	{
		PVSEntry& entry = pvsIt->second;
		DeletePendingRay(entry);
	}
}

bool CVisionMap::RayCastSubmit(const QueuedRayID& rayID, RayCastRequest& request)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	PendingRays::iterator it = m_pendingRays.find(rayID);
	assert(it != m_pendingRays.end());

	if (it != m_pendingRays.end())
	{
		PendingRayInfo& info = it->second;
		PVSEntry& entry = info.entry;

		const ObserverParams& observerParams = info.observerInfo.params;
		const ObservableParams& observableParams = entry.observableInfo.params;
		const Vec3& pos = observerParams.eyePos;

		request.pos = pos;
		request.dir = observableParams.pos[entry.currentPos] - pos;
		request.objTypes = COVER_OBJECT_TYPES;
		request.flags = HIT_COVER | HIT_SOFT_COVER; // TODO: Move this to the params struct
		request.maxHitCount = 1;

		size_t skipListCount = observerParams.skipListSize + observableParams.skipListSize;
		assert(request.skipListCount <= RayCastRequest::MaxSkipListCount);

		skipListCount = std::min<size_t>(skipListCount, RayCastRequest::MaxSkipListCount);
		request.skipListCount = skipListCount;

		size_t observerSkipListCount = std::min<size_t>(observerParams.skipListSize, RayCastRequest::MaxSkipListCount);
		memcpy(request.skipList, observerParams.skipList, observerSkipListCount * sizeof(IPhysicalEntity*));

		size_t observableSkipListCount = 
			std::min<size_t>(observableParams.skipListSize, RayCastRequest::MaxSkipListCount - observerSkipListCount);
		memcpy(request.skipList + observerSkipListCount, observableParams.skipList, 
			observableSkipListCount * sizeof(IPhysicalEntity*));

		return true;
	}

	return false;
}

void CVisionMap::RayCastComplete(const QueuedRayID& rayID, const RayCastResult& result)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	PendingRays::iterator it = m_pendingRays.find(rayID);
	if (it != m_pendingRays.end())
	{
		PendingRayInfo& info = it->second;
		PVSEntry& entry = info.entry;
		entry.pendingRayID = 0;

		bool visible = !result;

		const ObserverInfo& observerInfo = info.observerInfo;
		const ObservableInfo& observableInfo = entry.observableInfo;

		if (!visible)
		{
			if (++entry.currentPos < observableInfo.params.posCount)
			{
				entry.pendingRayID = QueueRay(info.observerID, observerInfo, entry);
				return;
			}
		}

		entry.currentPos = 0;

		if (entry.visible != visible)
		{
			entry.visible = visible;

			if (observerInfo.params.callback)
			{
				observerInfo.params.callback(info.observerID, observerInfo.params, entry.observableInfo.id,
					observableInfo.params, visible);
			}
			
			if (observableInfo.params.callback)
			{
				observableInfo.params.callback(info.observerID, observerInfo.params, entry.observableInfo.id,
					observableInfo.params, visible);
			}

			// check it has not been removed by the callback
			PendingRays::iterator eraseIt = m_pendingRays.find(rayID);
			if (eraseIt != m_pendingRays.end())
				m_pendingRays.erase(eraseIt);
		}
		else
			m_pendingRays.erase(it);
	}
}

void CVisionMap::AcquireSkipList(IPhysicalEntity** skipList, uint32 skipListSize)
{
	for (uint32 i = 0; i < skipListSize; ++i)
		skipList[i]->AddRef();
}

void CVisionMap::ReleaseSkipList(IPhysicalEntity** skipList, uint32 skipListSize)
{
	for (uint32 i = 0; i < skipListSize; ++i)
		skipList[i]->Release();
}

void CVisionMap::DebugDraw()
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

	if (gAIEnv.CVars.DebugDrawVisionMap > 1)
	{
		Observers::iterator it = m_observers.begin();
		Observers::iterator end = m_observers.end();

		for ( ; it != end; ++it)
		{
			ObserverID observerID = it->first;
			ObserverInfo& observerInfo = it->second;

			DebugDraw_ObserverPVS(observerID, observerInfo);
			DebugDraw_ObserverStats(observerID, observerInfo);
		}
	}

	DebugDrawStats();
}

void CVisionMap::DebugDrawStats()
{
	const float fontSize = 1.25f;
	const float x = 960.0f;
	float y = 200.0f;

	CDebugDrawContext dc;

	stack_string text;
	text.Format(
		"PVS Updates: %d\n"
		"VIS Updates: %d\n",
		m_pvsUpdateCount,
		m_visUpdateCount);

	dc->Draw2dLabel(x, y, fontSize, Col_LightSteelBlue, false, "%s", text.c_str());
}

void CVisionMap::DebugDraw_ObserverPVS(const ObserverID& observerID, const ObserverInfo& observerInfo)
{
	CDebugDrawContext dc;

	const ObserverParams& params = observerInfo.params;
	Vec3 pos = params.eyePos;
	Vec3 dir = params.eyeDir;

	dc->SetAlphaBlended(true);
	dc->SetBackFaceCulling(false);

	PVS::const_iterator it = observerInfo.pvs.begin();
	PVS::const_iterator end = observerInfo.pvs.end();
	
	for ( ; it != end; ++it)
	{
		const PVSEntry& entry = it->second;
		dir = entry.observableInfo.params.pos[entry.currentPos] - pos;
		
		if (entry.visible)
			dc->DrawLine(pos, ColorB(0xff00ff00u), pos + dir * 0.5f, ColorB(0x7f00ff00u));
		else
			dc->DrawLine(pos, ColorB(0xff0000ffu), pos + dir * 0.5f, ColorB(0x7f0000ffu));
	}

	dc->SetDepthWrite(false);

	if (gAIEnv.CVars.DebugDrawVisionMap > 2)
	{
		const uint32 pointCount = 32;

		Vec3 points[pointCount];
		Vec3 triangles[(pointCount - 1) * 2 * 3];

		Vec3 y(params.eyeDir.x, params.eyeDir.y, params.eyeDir.z);
		Vec3 x(y.Cross(Vec3Constants<float>::fVec3_OneZ).GetNormalized());

		float anglePrimary = cry_acosf(std::min<float>(params.primaryFoVCos, params.peripheralFoVCos)) * 2.0f;

		for(uint32 i = 0; i < pointCount; ++i)
		{
			float angle = ((float)i / (float)(pointCount - 1) - 0.5f) * anglePrimary;
			points[i] = y * cry_cosf(angle) + x * cry_sinf(angle);
		}

		float radius = params.sightRange;
		float inner = 0.25f;

		uint32 triangleVertexCount = 0;
		for(uint32 i = 0; i < pointCount - 1; ++i)
		{
			triangles[triangleVertexCount++] = pos + points[i] * inner;
			triangles[triangleVertexCount++] = pos + points[i + 1] * radius;
			triangles[triangleVertexCount++] = pos + points[i] * radius;

			triangles[triangleVertexCount++] = pos + points[i] * inner;
			triangles[triangleVertexCount++] = pos + points[i + 1] * inner;
			triangles[triangleVertexCount++] = pos + points[i + 1] * radius;
		}

		dc->DrawTriangles(triangles, triangleVertexCount, ColorF(Col_MediumSeaGreen, 0.5f));
	}
}

void CVisionMap::DebugDraw_ObserverStats(const ObserverID& observerID, const ObserverInfo& observerInfo)
{
	CDebugDrawContext dc;

	Vec3 pos = observerInfo.params.eyePos;
	pos.z += 0.2f;

	PVS::const_iterator pvsIt = observerInfo.pvs.begin();
	PVS::const_iterator end = observerInfo.pvs.end();

	int visible = 0;
	
	for ( ; pvsIt != end; ++pvsIt)
	{
		const PVSEntry& entry = pvsIt->second;
		if (entry.visible)
			++visible;
	}

	const char* priority = 0;
	ColorB prioColor = Col_White;

	switch (observerInfo.params.priority)
	{
	case eLowPriority:
		priority = "Low";
		prioColor = Col_Green;
		break;
	case eMediumPriority:
		priority = "Medium";
		prioColor = Col_Yellow;
		break;
	case eHighPriority:
		priority = "High";
		prioColor = Col_Orange;
		break;
	case eVeryHighPriority:
		priority = "Very High";
		prioColor = Col_Red;
		break;
	default:
		assert(false);
	}

	stack_string text;
	text.Format(
		"Visible: %d/%d\n"
		"Priority: %s",
		visible,
		observerInfo.pvs.size(),
		priority);

	const float fontSize = 1.15f;

	float x, y, z;
	if (!dc->ProjectToScreen(pos.x, pos.y, pos.z + 0.25f, &x, &y, &z))
		return;

	if ((z < 0.0f) || (z > 1.0f))
		return;

	x *= dc->GetWidth() * 0.01f;
	y *= dc->GetHeight() * 0.01f;

	dc->Draw2dLabel(x, y, fontSize, prioColor, true, "%s", text.c_str());
}
